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It  is  well-known  that  stand-alone  inertial  navigation  systems  (INS)  have  their 
errors  diverging  with  time.  The  traditional  approach  for  solving  such  incovenience 
is  to  resort  to  position  and  velocity  aiding  such  as  global  navigation  satellite  sys¬ 
tems  (GNSS)  signals.  However,  misalignment  errors  in  such  fusion  architecture  are 
not  observable  in  the  absence  of  maneuvers.  This  investigation  develops  a  novel 
sighting  device  (SD)  model  for  vision-aided  inertial  navigation  for  use  in  psi-angle 
error  based  extended  Kalman  filtering  by  means  of  observations  of  a  priori  mapped 
landmarks.  Additionally,  the  psi-angle  error  model  is  revisited  and  an  extended 
Kalman  filter  datasheet-based  tuning  is  explained.  Results  are  obtained  by  com¬ 
puter  simulation,  where  an  unmanned  aerial  vehicle  flies  a  known  trajectory  with 
inertial  sensor  measurements  corrupted  by  a  random  constant  model.  Position  and 
velocity  errors,  misalignment,  accelerometer  bias,  rate-gyro  drift  and  GNSS  clock 
errors  with  respect  to  ground-truth  are  estimated  by  means  of  INS/GNSS/SD 
fusion  and  tested  for  statistical  consistency. 

I.  Introduction 

Advances  in  microelectromechanical  inertial  sensors  (MEMs)  made  low-cost  inertial 
navigation  systems  (INS)  commercially  available.  On  the  other  hand,  their  errors  quickly 
diverge  with  time  and  set  an  upper  bound  on  the  duration  of  autonomous  operations 
and  thus  such  systems  become  improper  for  use  in  low-cost  unmanned  aerial  vehicle 
(UAV)  missions.  The  traditional  approach  for  solving  such  inconvenience  is  to  resort  to  a 
global  navigation  satellite  system  (GNSS)  receiver  as  position  and  velocity  aiding  device. 
Hence,  INS/GNSS  fusion  yields  bounded  navigation  errors.  However,  misalignment  errors 
in  such  fusion  architecture  are  not  observable  in  the  absence  of  maneuvers  [1,2].  In  the 
light  of  such  restriction,  the  present  study  develops  a  novel  model  for  INS/GNSS  and 
sighting  device  (SD)  integration  for  use  in  outdoor  navigation  with  known  landmarks. 
In  general,  outdoor  navigation  in  structured  environments  requires  some  sort  of  road¬ 
following.  Herein,  a  priori  mapped  landmarks  are  imaged  by  a  camera  and  tracked  in 
the  image  plane  to  aid  the  INS. 

An  initial  study  on  the  matter  was  conducted  in  [3],  which  developed  two  distinct 
strategies  for  INS/SD  fusion  by  means  of  psi-angle  error  model  [4]  based  extended  Kalman 
filtering  (EKF).  One  of  them  is  the  inspiration  for  this  paper  and  explores  a  relationship 
between  the  INS  errors  and  the  position  of  a  landmark  in  the  field  of  view  relative  to 
the  line  of  sight  (LOS)  of  the  SD,  after  the  latter  is  aimed  at  the  assumed  position  of 
the  landmark.  The  shortcoming  of  such  procedure  is  the  restriction  to  have  the  camera 
maintaining  LOS  pointing  to  the  landmark.  Such  restriction  has  been  recently  overcome 
by  means  of  resorting  to  a  generalized  SD  model  in  which  the  difference  between  measured 
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and  estimated  positions  of  the  tracked  landmark  in  the  plane  of  image  are  correlated  with 
INS  errors  [5]. 

However,  the  psi-angle  error  framework  has  received  only  a  modicum  of  attention  in 
INS/SD  integration  [6]  since  [3].  The  present  investigation  revisits  such  scheme,  which 
has  proven  sucessful  for  INS/GNSS  fusion  [7],  by  means  of  developing  an  INS/SD  fusion 
formulation  within  the  psi-angle  error  based  EKF  framework. 

Initially,  the  INS  psi-angle  error  model  is  revisited,  and  a  datasheet-based  nominal 
EKF  tuning  is  explored  alongside.  Ultimately,  an  INS/GNSS/SD  EKF-based  fusion 
strategy  is  proposed  for  the  estimation  of  navigation  and  sensors  errors,  and  evaluated 
by  means  of  Monte  Carlo  simulation  and  statistical  consistency  tests  [8] . 


II.  Reference  frames  and  Earth  model 


Reference  frames  and  the  Earth  model  are  here  briefly  discussed.  The  WGS-84  ellip¬ 
soid  has  been  used  due  to  its  accuracy  and  simplicity  [9].  The  local  reference  frames  at 
the  true  and  computed  positions  differ  [4],  and  are  respectively  denoted  by  T  =  {t1;  t2,t3} 
and  C  =  { C\ ,  c2 ,  c3 }  (see  figure  1). 


Figure  1.  Illustration  of  true  (P)  and  computed  (C)  positions;  and  platform  (P),  computed 
(C)  and  true  (T)  reference  frames. 

Additionally,  equally  important  frames  are  /,  E,  B  and  P,  respectively,  inertial,  Earth- 
fixed,  vehicle  body  and  platform  coordinate  systems.  The  latter  is  the  local  reference 
frame  computed  by  the  inertial  navigation  system  at  its  estimated  position  and  affected 
by  attitude  estimation  errors  [4]. 


III.  Mathematical  notation 


The  chosen  notation  [10]  is  illustrated  by  table  1. 

Furthermore,  the  decomposition  of  a  vector  v  G  M3  into  its  components  in  a  R  coor¬ 
dinate  system  is  denoted  by  means  of  the  right  subscript  position,  e.g. 


(1) 
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Table  1.  Kinematics  notation 


Notation 

Meaning 

px/  Y 

Position  of  point  X  w.r.t.  point  Y 

R 

INS  position  with  respect  to  Earth  centre 

Rins 

INS-computed  position  with  respect  to  Earth  centre 

R=  v 

e 

Terrestrial  velocity 

RlNS=  Vins 

INS-computed  terrestrial  velocity 

SS 

R 

Acceleration  w.r.t.  S  reference  frame 

UIxy 

Angular  velocity  of  X  coordinate 
frame  w.r.t.  Y  frame 

4 

Specific  force 

g(-) 

Earth  gravity  at  designated  point 

Di 

Direction  cosine  matrix:  rotates  from  A 
coordinate  frame  into  alignment  with  B  frame 

IV.  Kalman  filter  formulation 

For  the  purpose  of  EKF-based  INS/GNSS/SD  fusion,  the  INS  psi-angle  error,  GNSS 
receiver  and  camera  linear  models  are  outlined,  i.e. ,  linear  state  and  covariance  propaga¬ 
tion  and  update  are  formulated. 

A.  INS  psi-angle  error  model  revisited 

Consider  strapdown  accelerometers  and  rate-gyros  measurements  corrupted,  respectively, 
by  unknown  constant  bias  V  and  drift  e,  modelled  as  random  normal  variables  with  ay 
and  ue  standard  deviations.  Additionally,  additive  zero-mean  white  additive  noise  uiaccei 
and  uigyro  are  considered  with  craccei  and  agyro  standard  deviations.  The  measured  specific 
force  Asp^m  is  given  [4]  by  a  rotation  of  Asp  by  the  misalignment  vector  (see  figure  1) 
from  C  to  P  reference  frame,  and  biased  by  V  according  to 

Asp,m  =  V  T  Asp  ij)  x  Asp  T  ujaccei  (2) 

whereas  [4] 

Asp  =R  -g(R)  «nx(n  xR)  (3) 

and 

a 

AsPlm  =RlNS  -g(RlNs)  —  x  (O  X  RlNS )  (4) 

where  if),  g(R),  g{RiNS ),  R,  Rins  denote,  respectively,  the  misalignment  rotation 
vector  from  the  computed  to  the  platform  reference  frame,  Earth  gravity  at  the  true 
and  computed  positions,  Earth  angular  velocity,  and  true  and  computed  positions  with 
respect  to  the  Earth  centre. 
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If  Sp  and  Sv  are  defined  as  INS  computed  errors  in  position  and  velocity,  i.e., 


Sp  —  Rins  ~  R 

Sv  =  VlNS  —  V 


then  equations  2,  3  and  4  imply 

a  ** 

v  -  x/j  X  Asp  +  u )accei  =Sp  -  [g(RiNs)  ~  g(R)]-Sl  x  (12  x  Ap) 

s - v* - ' 

$9 

which  can  be  rewritten  as 


(5) 


(6) 


/  ie  i  \ 

V  -  x  Asp  +  u;acce*  =  (Sp  +12x  Sp  )  —  Sg  —  ft  x  (fl  x  Sp)  = 

=Sv  +12x  <5p  — <5g  —  12  x  (12  x  <5p)  =(5v  +u;c*  x  <5n  +  12  x  Sv  —  Sg  (7) 


Ultimately, 

Sv=  Sg  -  (212  +  cjce)  x  Sv  +  Asp  x  +  V  +  u;acce;  (8) 

On  the  other  hand,  the  derivative  of  Sp  with  respect  to  frame  C  yields 

Sp=Sp  —u ;ce  x  Sp  =  Sv  —  u>ce  x  Sp  (9) 

Additionally,  it  can  be  shown  that  [4] 

i 

6  lj}  UJgyrQ  (10) 

hence 

—UJCl  X  Xj>  =  —  e  —  (12  +  U)Ce )  X  xj)  —  (jj  gyro  (11) 

Finally,  according  to  [4],  assuming  Vg  and  eB  are  random  constants,  representing 
vector  equations  8,  9  and  11  in  the  C  coordinate  frame,  and  then  employing  the  Dp 
estimated  by  the  INS,  the  psi-angle-based  INS  error  dynamics  model  is  formulated  as 


Spc  =  Svc  -  [ijJ^x}Spc  (12) 

Svc  =  Sgc  —  [(212c  +  ijJc')'x\^vc  +  [DpAsp^rn,B'x\'4,c  +  Dp^  b  +  DpivacceiB  (13) 
V’C  =  —  [(^C  +  u}<c)  X\^C  ~  DpEB  —  DpLVgyroB  (14) 

V  b  =  03x1  (15) 

£b  =  03xi  (16) 

where  the  notation  [Crx]  means 

0  ~Cr3  Cr2 

[C-RX]  —  Cr-3  0  —  Crl  (17) 

Cr2  Crl  0 


311 


and  Sgc  is  approximated  by  [11] 


8gc 


geRl 

(Re  +  hcf 


f-l\ 

-1 

V2/ 


(18) 


with  ge  and  Re  denoting,  respectively,  the  gravity  and  the  radius  of  the  Earth  as  computed 
with  the  WGS-84  Earth  model,  at  the  geographic  location  of  the  INS  sensors,  according 
to  [11] 

Re  =  f?o(l  —  esin2  Ac)  (19) 

and 

ge  —  (1  +  0.0053sm2Ac)<?0  (20) 

where  R0,  e,  go  and  Ac  denote,  respectively,  the  Earth  equatorial  radius,  eccentricity  and 
equatorial  gravity,  and  the  INS-computed  latitude.  The  Earth  model  parameters  are 
found  in  [9]. 

In  the  light  of  the  foregoing  development,  it  is  convenient  to  define  the  EKF  state 
vector  as 

Xekf  =  {fipc  $vc  c  eB  c^t)  (21) 

where  cAt  denotes  the  random  constant  GNSS  clock  error  model  with  crc/\t  standard 
deviation,  whose  compensation  is  fundamental  in  INS/GNSS  integration  [11], 

The  zero-order  hold  (ZOH)  discretization  of  equations  12,  13,  14,  15  and  16  yields  the 
error-state  transition  matrix  Fk  and  process  noise  covariance  Qk  for  the  EKF.  In  practice, 
the  discretization  which  yields  Qk  is  [12] 


Qk  —  Gk 


^ accel^3x3  ^3x3 


^3x3  ® gyro ^ 


3x3 


A  tGl 


(22) 


where 


Gk  = 


03x3 

03x3 

DBP 

03x3 

03x3 

_ V)B 

Up 

07x3 

07x3 

(23) 


and  At  denotes  the  discretization  sample  time.  Finally,  EKF  tuning  is  addressed  with 
inertial  sensors’  datasheet  specifications. 


B.  GNSS  tightly  coupled  integration 

GNSS  integration  equations  are  listed  in  the  following  without  further  explanations  due  to 
the  broad  extension  of  available  literature  on  the  matter  [7].  Integration  is  performed  in 
a  tightly  coupled  architecture  involving  pseudorange  and  deltarange  measurements.  For 
each  satellite  Si,  pseudorange  and  deltarange  innovations  are  incorporated  by  means  of 
equations  24  and  25,  where  Ui  denotes  the  estimated  line-of-sight  (LOS)  unit  vector  from 
the  user’s  receiver  antenna  to  satellite  Si.  Pseudorange  and  deltarange  measurements  are 
corrupted  by  additive  Gaussian  noise  ~  N(0,ap)  and  u>ghV~N(0,  crv).  The  antenna 
lever  arm  has  been  considered  as  exactly  compensated.  Furthermore,  GNSS  clock  error 
dynamics  is  modelled  according  to  equation  26.  In  practice,  a  more  complex  model  that 
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accounts  for  clock  drift  should  be  implemented  [7].  However,  for  the  sake  of  simplicity, 
the  present  work  implements  a  simpler  model  and  focuses  on  camera  integration. 


pSi/F  -  pSi/c  =  iii  ■  Sp  +  cAt  +  ujSi)P 

eSi/P  eSi/C 

p  -  p  =  iii  ■  Sv  +  u )Si,v 


(24) 

(25) 


4 cAt  =  0  (26) 

at 


C.  Sighting  device 

As  previously  stated,  the  basic  idea  of  the  proposed  INS/SD  fusion  architecture  is  based 
upon  tracking  mapped  landmarks  L,;  G  L,i  =  1..A),  each  one  with  a  priori  known  LLA 
coordinates  (latitude,  longitude,  altitude).  For  each  L7l  its  position  with  respect  to  the 
camera  V,  pLi/v ,  (see  figure  2)  is  described  in  the  V  coordinate  frame  as  [5] 

Pv'r  =  -  Pe)  -  pT)  (27) 


Figure  2.  Perspective  projection  geometry  in  the  plane  of  image.  Adapted  from  [5]. 


The  camera  V  is  assumed  installed  next  to  the  inertial  sensors’  position  P  and  has 
its  axes  aligned  according  to  figure  3,  thus 


pLv‘/v  =  Df,DTBDppLE-  -  p^) 


(28) 


Hence  the  adimensional  normalized  measurement11  zl.  =  ( Wi,hi )  provided  by  the 
camera  is  given  by 

Li/V 

zLi  —  hi? - - TTT7  +  ^ cam  (29) 


1  0  0 


pLv‘,V 


aIt  is  assumed,  without  loss  of  generality,  camera  focal  length  /  =  1. 
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Figure  3. 


Definition  of  camera  coordinate  system  (top  view). 


where  u )cam  G  M(M)2xi  is  white  gaussian  noise  with  standard  deviation  acam,  and  II  is 
defined  by 


n 


o  1  o 
o  o  1 


(30) 


<jj cam  shall  be  modelled  to  account  for  uncertainties  in  target  tracking  algorithms, 
servomechanism  control  and  misalignment  between  camera  and  vehicle  bodyb.  The  es¬ 
timated  normalized  measurement  vector  is  estimated  by  means  of  the  navigation  algo¬ 
rithm  [13]  computed  variables  (i.e. ,  position,  velocity  and  attitude)  by 


zLt  =  nT 


Li/C 

Pvc 


1  0  0 


vLi/C 

Pvc 


(31) 


where  Vc  is  the  computed  camera  reference  frame, 
expressed  as 


Li/C 

Pvc 


=  D^DcbDE(ple‘ 


Similarly  to  equation  28,  PyJC  is 
- P°e )  (32) 


The  orientation  of  the  camera  V  in  relation  to  the  vehicle  body  B  is  assumed  known 
with  great  accuracy,  thus  D^-  =  Dp.  In  addition,  notice  that  INS  attitude  estimates  are 
employed  and  thus  follows  the  approximation  D^c  «  Dp.  In  this  scope,  equation  32  can 
be  rewritten  as 

p ii,C  =  D^Dg(pLE‘ -  p%)  (33) 

Notice  that  all  terms  in  the  right-hand  side  of  equation  33  are  available  from  INS 
navigation  algorithm  and  can  be  readily  computed.  For  use  in  the  EKF,  the  difference 
rLi  =  ZLi  —  ZLt,  and  how  it  relates  to  navigation  errors,  is  explored  in  the  following. 

It  has  been  assumed  that  r^,.  is  function  solely  of  dp  and  i/y  disregarding  clectrooptical 
distortions  in  the  sighting  device.  Hence,  the  Jacobian  J  =  is  in  the  sparse  form 


(34) 


b  Servomechanism  control  and  misalignment  between  camera  and  vehicle  body  errors  usually  cannot 
be  accurately  modelled  as  white  gaussian  noise  hence  inflation  in  Kalman  filter  noise  covariance  statistics 
should  take  place  in  practice 


J  = 


dSpc 


J  2x3 


d*Li 

dike 


J'2x7 
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Ozl 

First,  is  calculated  noticing  that  equation  33  can  be  rewritten  as 


Li/C 


Pvc  ~  DvDbDc{Pe  ~  Pe)  ~  Dv(DBDTDc)(DcDT)(pE’  —  pE)  ~ 

«  D$Dl(I3x 3  +  [^cx])^(pJ  -pcE)  =  DBDTB{I3x 3  +  [^x])^(p^-p^  -  SpE)  = 
=  D^DlD^-p^)  +  D^Dl[^cx]D^-p^)-D^Dl(I3x3  +  [^cx])D^Spc  « 


Py/F  +  -  p£)  -  DyDB(I3x3  +  [S9cx])SPc  ~ 

«  Pv /V  +  JD^I[^cx]JD|(p^  -  p£)  -  DyD^Spc  (35) 

9ZL; 


Therefore,  considering  the  partial  derivative 
deliver 


Pvf  =Pvi/V  -D^DlSp 


dSpc  at  point  t/^c  =  0,  above  equations 

(36) 


c 


For  the  computation  of  zL.  is  rewritten  as 

Zl  ,num 


Hence, 


ZU  = 

Z\  ,num 
^2  ,num. 

^ den 

dir 


Zi  = 


Z2,r 


0  1 
0  0 
0  0 


Zden 
Li/V 


Zden 


iPv  '  ~  DyDBSPc) 

(Pvi/V  -  DyDTB6pc ) 

(Pv/V  ~  DyDB6pc) 


where 


and 


dzk 


06p, 


ddpc 


'  ( Zden 


dzi 
dip  d 
dz2 
L55pci 


dzi 

dSpc2 

dz2 


dzi 
dSpc  3 
dz2 


dz. 


k.num 


dSpc2  dSpc 3 . 

dZden 


cj 


* den 


05  p, 


%k,r. 


cj 


1 05p, 


), 


CJ 


k  =  1..2 
3  =  1-3 


/  dz\  ,nurr 
'  dSpd 
dz2  ,nurr 


V 


dSpci 
9z<len 
dSpd 

/  dzi  ,nurr 

'  95pc2 

dz2  ,nurr 


V 


dSpc2 
9z<len 
dSpc2 

/  dzi  ,nurr 
'  98pc3 

9z2  ,nurr 


V 


98pc3 

9z<len 

98pc3 


"o 

1 

o" 

"l" 

=  — 

0 

0 

1 

db  dt 

0 

1 

0 

0 

0 

"o 

1 

o" 

"o" 

=  — 

0 

0 

1 

db  dt 

1 

1 

0 

0 

0 

"o 

1 

o" 

"o" 

=  — 

0 

0 

1 

db  dt 

0 

1 

0 

0 

1 

(37) 

(38) 

(39) 

(40) 

(41) 

(42) 

(43) 

(44) 

(45) 


Above  computations  deliver  the  first  2x3  block  of  the  Jacobian  in  equation  34.  Simi¬ 
larly,  for  the  computation  of  the  remaining  nonzero  block,  notice  that  the  xp  misalignment 
affects  the  camera  measurement  (for  small  angles)  according  to 


Li/C 

Pvc 


Pv  '  +  dvD1[iPcx]Dt{Pe  -Pe) 
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(46) 


By  means  of  equation  46  and  the  same  strategy  used  for  the  computation  of  the 
Jacobian  with  respect  to  position,  follows 


7i  ,num  72  ,num 

7i  =  — - »  72  =  — - 

H I  den  H I  den 


(47) 


Tl,  num 


r)flfnum 


0  1  0 
0  0  1 


t Pv‘,V 


(p 


Li/V 

V 


+  D»DTBWcxmPLE‘ 

+  D*Dl[iJ>cx]D$(p% 


Pe )) 
Pe)) 


(48) 

(49) 


7  den 


1  0  0 


ip 


Li/V 

V 


+  D*Di 


[^cx]Dt(Pe  -  Pe)) 


(50) 


dzLi 

d'ipe 


971 

971 

971 

dipd 

dlpc2 

dlpc3 

972 

972 

972 

.dipd 

dlpc2 

dipc  3. 

(51) 


<97  fc 

di/jCj 


lien 


(l 


den~ 


<97  k,r 


dp>, 


7 


e? 


<97de 

1  dip, 


7 


k  =  1..2 
3  =  1-3 


V 


dipd 

d*)2,nun 

dipd 

d'Yd.en 


dipd 


/ 


/  ^7l,wum  \ 


V 


dlpc2 
972, nun 
dlpc2 
d^jden 


dlpc.2 


) 


/  d^l^num  \ 


dlpc3 
d~f2,nur 
d^c3 
^Kden 
<  dlpc3 


0 

1 

0 

0 

0 

1 - 

0 

= 

0 

0 

1 

db  dp 

0 

0  ■ 

-1 

1 

0 

0 

0 

1 

0 

’0 

1 

0" 

’  0 

0 

1" 

= 

0 

0 

1 

db  dp 

0 

0 

0 

1 

0 

0 

-1 

0 

0 

"0 

1 

0" 

’0 

-1 

0" 

= 

0 

0 

1 

db  dp 

1 

0 

0 

1 

0 

0 

0 

0 

0 

ELt/C 


VcPe 


dEpLe/c 


el,/c 


DcPe 


(52) 


(53) 


(54) 


(55) 


Above  computations  deliver  the  Jacobian  J  which  quantitatively  describes  how  navi¬ 
gation  errors  affect  camera  pointing  and  is  used  as  sensor  model  in  the  extended  Kalman 
filter. 


V  L,  ~  J XeKF  +  <-dcam 


(56) 


V.  Simulation  results 

Consider  a  simulated  scenario  where  an  unmanned  aerial  vehicle  (UAV),  initially 
positioned  at  LLA  =  (0,  0, 100m),  equipped  with  3-axis  sensitive  triads  of  strapdown  rate- 
gyros  and  accelerometers,  a  GNSS  receiver  and  a  camera  flies  a  trajectory  with  cruising 
speed  300 m/s  toward  North  and  altitude  100m.  Sensors  specifications  are  illustrated  by 
table  2. 

GNSS/SD  updates  were  made  at  100ms  intervals.  After  the  instant  t  =  5 sec,  EKF 
estimated  navigation  variables  and  sensors  errors  are  used  for  in-flight  correction  of  the 
INS  computed  position,  velocity  and  attitude  and  to  calibrate  the  inertial  sensors  and 
GNSS  receiver  clock  error.  INS  correction  is  of  utmost  importance  in  such  systems  due 
to  the  linearization  of  the  INS  error  dynamics  at  the  computed  INS  navigation  solution. 
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Table  2.  Sensors  Imperfections 


INS 

&accel 

Uv 

& gyro 

1  mg 

25  mg 

1  °/h 

25  °/h 

GNSS/SD 

&cAt 

300m 

(Jv 

0.01  m/s 

(Jp 

15  m 

O cam 

0.01 

With  respect  to  the  INS  navigation  algorithm,  [13]  and  [14]  provide  cost-effective 
multiple-rate  integration  methods  to  compute  position  and  velocity,  and  attitude,  re¬ 
spectively.  However,  such  algorithms  incur  in  errors  in  position,  velocity  and  attitude, 
commonly  known  in  the  literature  as  scrolling,  sculling  [16]  and  coning  [15].  Scrolling 
errors  added  to  the  position  channel  process  noise  (see  equation  22)  precludes  EKF  op¬ 
timality,  and  hence  ad-hoc  Qk  inflation  takes  place  in  the  following  manner 

hx3  03x13 
0l3x3  0i3Xi3 

On  top  of  that,  the  initial  extended  Kalman  filter  covariance  matrix  P(0|0)  is  also 
inflated.  Such  procedure  is  commonplace  in  real  applications  since  initial  errors  are  often 
not  known.  This  practice  yields  a  non-optimal  pessimist  filter  initiation  which  can  be 
detected  by  substantial  differences  in  root  mean  squared  (RMS)  estimation  and  EKF 
computed  covariance  during  the  KF’s  early  working  stages  as  can  be  seen  later  on. 

The  impact  of  the  number  of  available  landmarks  on  INS/GNSS/SD  system  perfor¬ 
mance  will  be  evaluated  by  comparing  single-sided  and  double-sided  observation  scenar¬ 
ios.  Only  one  landmark  update  is  made  at  each  updating  step  of  the  EKF.  In  the  case 
of  single-sided  observations,  only  landmark  Li  is  considered  in  the  filter  update.  In  the 
double-sided  observation  mode,  L i  and  L-2  are  alternately  available  for  the  update  stage. 
Landmarks  are  defined  in  table  3.  The  vehicle’s  trajectory  and  the  observed  landmarks 
are  illustrated  in  figure  4. 

Table  3.  Landmarks  Position  Coordinates 


Qk  t—  Qk  +  10  1A t 


Landmark 

Latitude  (deg) 

Longitude  (deg) 

Altitude  (m) 

1 

io-2 

50  x  10”1 

10 

2 

50  x  10’1 

10"2 

10 

For  the  sake  of  simplicity,  constant  visibility  to  4  GNSS  satellites  is  assumed,  each 
with  a  fixed  position  with  respect  to  Earth  during  the  simulation  time  interval.  LLA 
satellite  coordinates  are  given  in  table  4. 

The  evaluation  of  INS/GNSS/SD  fusion  algorithm  performance  with  single  or  double¬ 
sided  observations  is  based  on  a  Monte  Carlo  simulation  [8]  with  50  realizations  and 
two  statistical  tests.  These  are  the  normalized  estimation  error  squared  (NEES)  and 
normalized  innovation  squared  (NIS),  which  are  described  in  [8]  and  used  hereafter  with 
a  5%  alarm  rate.  The  number  of  realizations  is  chosen  so  that  a  balance  between  reliable 
statistical  study  and  practicable  simulation  time  is  obtained. 
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Vehicle  trajectory  and  Landmark  positions 
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4 
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Figure  4.  Vehicle  trajectory  and  landmark  positions  (top  view). 


Table  4.  GNSS  Satellites  Position  Coordinates 


Satellite 

Latitude  (deg) 

Longitude  (deg) 

Altitude  (km) 

1 

20 

-20 

20,000 

2 

40 

-20 

20,000 

3 

-30 

40 

20,000 

4 

-25 

30 

20,000 

w 


■  Vehicle  Trajectory 

-  Landmark  1 

-  Landmark  2 


□ 


2  3  4 

North  (m) 


A.  Single-sided  observations 

Figure  5  displays  INS/GNSS/SD  fusion  with  single-sided  observations  EKF  computed 
covariances  and  root  mean  squared  (RMS)  estimation  errors  (denoted  by  dp,  dv,  tj),  V, 
e  and  cAt)  for  each  component  of  xekf ■  Additionally,  NEES  and  NIS  consistency  tests 
are  shown  with  the  corresponding  alarm  limits. 

Notice,  in  sharp  contrast  with  INS/GNSS  fusion,  the  observability  of  -0ci  and  ^c3 
without  resorting  to  maneuvers.  Notwithstanding,  ^c2  is  weakly  observable  due  to  land¬ 
mark  L\  location  East  of  the  vehicle.  It  is,  indeed,  intuitive  to  expect  the  inadequacy 
of  sighting  devices  to  yield  attitude  information  about  the  LOS  axis  due  to  the  assumed 
punctual  nature  of  the  landmark  projection  on  the  image  plane.  Similarly,  Vm  is  weakly 
observable. 

B.  Double-sided  observations 

Figure  6  displays  INS/GNSS/SD  fusion  with  double-sided  observations  EKF  computed 
covariances  and  RMS  estimation  errors  for  each  component  of  Xekf-  Additionally,  NEES 
and  NIS  consistency  tests  are  shown  with  the  corresponding  alarm  limits. 

The  addition  of  a  geometrically  favorable  landmark,  namely  L2,  positioned  North  of 
the  vehicle,  enhances  -0C 2  and  V&i  observability.  Thus,  the  RMS  error  quickly  diminishes 
and  filter  tuning  is  accomplished  as  far  as  the  NEES  and  NIS  tests  can  evaluate. 

Furthermore,  this  work  suggests  as  future  work  further  investigation  on  the  impact  of 
number  and  geometry  of  landmarks  on  Kalman  filter  observability,  which  can  be  analyt¬ 
ically  performed  in  a  fashion  similar  to  [1,2]. 
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EKF  computed  covariances,  RMS  estimation  errors  and  consistency  tests  for  INS/GNSS/SD  single-sided  observations 


4 


0.1 
008 
|  0  06 
>“  0  04 
:)  02 


-  RMS  estimation  errors 
_  3  o  EKF  computed  covariance 


200 


0.08 
|  0.06 
>8  0.04 
0.02 
% 


200  400 


0.015 

3 

0.01 


0  005-.V...  ....  .... 
% - 


8000 

8 

3  6000 

“  4000 
3-° 

2000 

% 


6000 

4000 

2000 


«  6000 
4000 
2000 


200 


c 

2 

0  4 

—  NEES 
Upper  L 

mil 

.. 

V.'.' 

18  N|S 
-  Limits 


Figure  5.  EKF  computed  covariances,  RMS  estimation  errors  and  consistency  tests  for 
INS/GNSS/SD  single-sided  observations. 


As  a  last  remark,  the  EKF  process  noise  in  the  position  channel  must  be  inflated 
according  to  equation  57  to  accomplish  NEES  and  NIS  statistical  consistency.  Otherwise, 
the  resulting  small  EKF  computed  covariance  and  the  corresponding  RMS  error  become 
statistically  inconsistent,  which  may  render  the  estimation  process  unreliable.  The  tuning 
of  the  position  channel  noise  to  reach  statistical  consistency  in  the  NEES  test  can  be 
addressed  by  self-tuning  algorithms  [17,18]. 
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Figure  6.  EKF  computed  covariances,  RMS  estimation  errors  and  consistency  tests  for 
INS/GNSS/SD  double-sided  observations. 


VI.  Conclusions 

An  imaging  measurement  model  is  formulated  for  use  in  a  psi-angle  error  based  ex¬ 
tended  Kalman  filter  (EKF)  that  yields  the  fusion  of  global  navigation  satellite  observ- 
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ables  with  vision-aided  inertial  navigation.  At  first,  the  EKF  uses  datasheet-based  nom¬ 
inal  tuning  and  the  estimation  performance  is  evaluated  by  means  of  Monte  Carlo  simu¬ 
lation.  The  resulting  performance  motivates  the  use  of  a  process  noise  inflation  scheme 
to  attain  statistical  consistency.  Position  and  velocity  errors,  misalignment,  accelerom¬ 
eter  bias,  rate-gyro  drift  and  GNSS  clock  errors  with  respect  to  ground-truth  are  then 
effectively  estimated  and  pass  the  tests  for  statistical  consistency.  Ultimately,  in-flight 
INS  correction  and  the  calibration  of  inertial  sensors  and  GNSS  receiver  clock  error  are 
successfully  accomplished. 
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